#include "OOBB.h"
#include "TestState.h"
#include "RBContactData.h"
#include "Game.h"

void CalculateOOBBCorners(const OOBB2& oobb, Vec2f f[])
{
	f[0] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1]);
	f[1] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1] * -1);	
	f[2] = oobb.centre + (oobb.axis[0] * oobb.extents[0] * -1) + (oobb.axis[1] * oobb.extents[1]);	
	f[3] = oobb.centre + (oobb.axis[0] * oobb.extents[0] * -1) + (oobb.axis[1] * oobb.extents[1] * -1);		
}

void CalculateOOBBCorners(const OOBB2& oobb, BoxPoints2& bp)
{
/*	bp.corners[0] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1]);
	bp.corners[1] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1] * -1);	
	bp.corners[2] = oobb.centre + (oobb.axis[0] * oobb.extents[0] * -1) + (oobb.axis[1] * oobb.extents[1]);	
	bp.corners[3] = oobb.centre + (oobb.axis[0] * oobb.extents[0] * -1) + (oobb.axis[1] * oobb.extents[1] * -1);*/		

	bp.corners[0] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1]);
	bp.corners[1] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) - (oobb.axis[1] * oobb.extents[1]);	
	bp.corners[2] = oobb.centre - (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1]);	
	bp.corners[3] = oobb.centre - (oobb.axis[0] * oobb.extents[0]) - (oobb.axis[1] * oobb.extents[1]);
}

float Dot(const Vec2f& v1, const Vec2f& v2)
{
	return float(v1.x * v2.x + v1.y * v2.y);
}

bool CheckCollision(const OOBB2& oobb1, const OOBB2& oobb2)
{
	// get 4 axis
	Vec2f axis[] =
	{
		oobb1.axis[0],
		oobb1.axis[1],
		oobb2.axis[0],
		oobb2.axis[1]
	};

	BoxPoints2 bp1, bp2;

	CalculateOOBBCorners(oobb1, bp1);
	CalculateOOBBCorners(oobb2, bp2);
	
	// for each axis
	float min1, min2, max1, max2;
	for (int i = 0; i != 4; i++)
	{
		float dp1 = Dot(axis[i], bp1.corners[0]);
		min1 = dp1;
		max1 = dp1;
		float dp2 = Dot(axis[i], bp2.corners[0]);
		min2 = dp2;
		max2 = dp2;

		for (int j = 0; j != 4; j++)
		{
			// project box 1
			// get min and max
			float dp1 = Dot(axis[i], bp1.corners[j]);
			if (dp1 < min1)
			{
				min1 = dp1;
			}
			if (dp1 > max1)
			{
				max1 = dp1;
			}
			// same for box 2
			float dp2 = Dot(axis[i], bp2.corners[j]);
			if (dp2 < min2)
			{
				min2 = dp2;
			}
			if (dp2 > max2)
			{
				max2 = dp2;
			}
		}
		// check if they overlap 
		// min1 < max2 && min2 < max1
		if (!(min1 < max2 && min2 < max1))
		{
			return false;
		}
	}
	return true;
}

/*
Complex Complex::Conjugate()
{
	return Complex(x, -y);
	or
	return Complex(real, -imaginary);
}

// returns the inverse rotation
Quaternion Quaternion::Conjugate()
{
	return Quaternion(m_w, -m_x, -m_y, -m_z);
}

Quaternion conj = myQuaternion.Conjugate

// returns the rotated vec3f pos
Vec3f RotateOOBB(Vec3f v)
{
	Quaternion vAfter = *this * Quaternion(0, v.x, v.y, v.z) * q.Conjugate();
	return Vec3f(vAfter.x, vAfter.y, vAfter.z);
}
*/

void CalculateOOBBCorners(const OOBB3& oobb, BoxPoints3& bp)
{
	bp.corners[0] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1]) + (oobb.axis[2] * oobb.extents[2]);
	bp.corners[1] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1]) - (oobb.axis[2] * oobb.extents[2]);	
	bp.corners[2] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) - (oobb.axis[1] * oobb.extents[1]) + (oobb.axis[2] * oobb.extents[2]);	
	bp.corners[3] = oobb.centre + (oobb.axis[0] * oobb.extents[0]) - (oobb.axis[1] * oobb.extents[1]) - (oobb.axis[2] * oobb.extents[2]);
	bp.corners[4] = oobb.centre - (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1]) + (oobb.axis[2] * oobb.extents[2]);
	bp.corners[5] = oobb.centre - (oobb.axis[0] * oobb.extents[0]) + (oobb.axis[1] * oobb.extents[1]) - (oobb.axis[2] * oobb.extents[2]);	
	bp.corners[6] = oobb.centre - (oobb.axis[0] * oobb.extents[0]) - (oobb.axis[1] * oobb.extents[1]) + (oobb.axis[2] * oobb.extents[2]);	
	bp.corners[7] = oobb.centre - (oobb.axis[0] * oobb.extents[0]) - (oobb.axis[1] * oobb.extents[1]) - (oobb.axis[2] * oobb.extents[2]);
}

float Dot(const Vec3f& v1, const Vec3f& v2)
{
	return float(v1.x * v2.x + v1.y * v2.y + v1.z * v2.z);
}

Vec3f CrossProduct(const Vec3f& u, const Vec3f& v)
{
	Vec3f result(0, 0, 0);
	result.x = u.y * v.z - u.z * v.y;
	result.y = u.z * v.x - u.x * v.z;
	result.z = u.x * v.y - u.y * v.x;

	return result;
}

bool CheckCollisionOOBB(const OOBB3& oobb1, const OOBB3& oobb2)
{
	// get 15 axis into an array
	D3DXVECTOR3 allAxis[] =
	{
		oobb1.axis[0],
		oobb1.axis[1],
		oobb1.axis[2],

		oobb2.axis[0],
		oobb2.axis[1],
		oobb2.axis[2],
		
		D3DXVECTOR3(),
		D3DXVECTOR3(),
		D3DXVECTOR3(),

		D3DXVECTOR3(),
		D3DXVECTOR3(),
		D3DXVECTOR3(),

		D3DXVECTOR3(),
		D3DXVECTOR3(),
		D3DXVECTOR3()
	};
	
	
	D3DXVec3Cross(&allAxis[7], &oobb1.axis[0], &oobb2.axis[0]);
	D3DXVec3Cross(&allAxis[8], &oobb1.axis[0], &oobb2.axis[1]);
	D3DXVec3Cross(&allAxis[9], &oobb1.axis[0], &oobb2.axis[2]);

	D3DXVec3Cross(&allAxis[10], &oobb1.axis[1], &oobb2.axis[0]);
	D3DXVec3Cross(&allAxis[11], &oobb1.axis[1], &oobb2.axis[1]);
	D3DXVec3Cross(&allAxis[12], &oobb1.axis[1], &oobb2.axis[2]);

	D3DXVec3Cross(&allAxis[13], &oobb1.axis[2], &oobb2.axis[0]);
	D3DXVec3Cross(&allAxis[14], &oobb1.axis[2], &oobb2.axis[1]);
	D3DXVec3Cross(&allAxis[15], &oobb1.axis[2], &oobb2.axis[2]);


	BoxPoints3 bp1, bp2;

	CalculateOOBBCorners(oobb1, bp1);
	CalculateOOBBCorners(oobb2, bp2);

	float min1 = 0, min2 = 0, max1 = 0, max2 = 0;
	// for each axis
	for (int axis = 0; axis != 15; axis++)
	{
		float dp1 = D3DXVec3Dot(&allAxis[axis], &bp1.corners[0]);
		min1 = dp1;
		max1 = dp1;
		float dp2 = D3DXVec3Dot(&allAxis[axis], &bp2.corners[0]);
		min2 = dp2;
		max2 = dp2;

		// project box 1 onto the axis
		// project box 2 onto the axis
		// do this by looping over the box corners
		for (int corners = 0; corners != 8; corners++)
		{
			// take a corner of box1 and project it onto the current axis
			// then do a corner of box2
			// this gives a float for each corner
			float  projectedCornerForBox1 = D3DXVec3Dot(&bp1.corners[corners], &allAxis[axis]);

			if (projectedCornerForBox1 > max1)
			{
				max1 = projectedCornerForBox1;
			}
			if (projectedCornerForBox1 < min1)
			{
				min1 = projectedCornerForBox1;
			}
			
			float  projectedCornerForBox2 = D3DXVec3Dot(&bp2.corners[corners], &allAxis[axis]);

			if (projectedCornerForBox2 > max2)
			{
				max2 = projectedCornerForBox2;
			}
			if (projectedCornerForBox2 < min2)
			{
				min2 = projectedCornerForBox2;
			}
		}

		// do the ranges overlap?
		if (min1 <= max2 && min2 <= max1)
		{
			// they do overlap therefore not a seperating axis
			// goes round and loops again
		}
		else
		{
			// if they don't, this is the seperating axis and return false
			return false;
		}
	}

	return true; // they do collide
}

void OOBB3::Draw()
{
	TheObjMeshManager::Instance()->Translate("crate_textured", centre);
//	TheObjMeshManager::Instance()->Translate("crate_textured", TheGame::Instance()->GetMaze()->GetPos());
	TheObjMeshManager::Instance()->Scale("crate_textured", extents[0] * 2.0f, 
														extents[1] * 2.0f,
														extents[2] * 2.0f);
	TheObjMeshManager::Instance()->Draw("crate_textured");
}